#ifndef MAP_BUILDER_H_
#define MAP_BUILDER_H_

#include <rclcpp/rclcpp.hpp>
#include <nav_msgs/msg/occupancy_grid.hpp>
#include <sensor_msgs/msg/point_cloud2.hpp>
#include "mapping/2d/probability_grid_range_data_inserter.h"
#include "mapping/2d/probability_grid.h"
#include "mapping/sensor/range_data.h"

namespace cartographer {
namespace mapping {
class MapBuilder : public rclcpp::Node {
public:
    // 构造函数：初始化MapBuilder对象
    // 参数initial_map_size：初始地图大小，默认值为2000
    // 参数resolution：地图分辨率，默认值为0.1
    MapBuilder(int initial_map_size = 2000, float resolution = 0.1);

    void LaserCloudHandler(const sensor_msgs::msg::PointCloud2::SharedPtr msg);

    // 更新地图数据
    // 参数range_data：传感器的测距数据，用于更新概率网格
    void UpdateMap(const sensor::RangeData& range_data);

private:
    // 发布occupancy grid（占用网格）的ROS2 publisher
    rclcpp::Publisher<nav_msgs::msg::OccupancyGrid>::SharedPtr p_occupancy_grid_pub_;
    // 存储占用网格数据的消息
    nav_msgs::msg::OccupancyGrid occupancy_grid_;

    rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr p_sub_laser_cloud_;

    // probability grid
    // 概率地图指针，用于计算和存储环境的占用概率
    std::unique_ptr<ProbabilityGrid> p_probability_grid_;
    // 概率地图数据插入器指针，用于将测距数据插入到概率网格中
    std::unique_ptr<ProbabilityGridRangeDataInserter2D> p_probability_grid_range_data_inserter_;
};

}  // namespace mapping
}  // namespace cartographer

#endif